
import time
## 舵机控制
from board import SCL, SDA
from busio import I2C
from adafruit_motor import servo
from adafruit_pca9685 import PCA9685
# 超声波测距
from ultrasound import Ultrasound
# 数据处理
import numpy as np
import pandas as pd
from draw_lida import draw

# pca控制板
pca = PCA9685(I2C(SCL, SDA))
pca.frequency = 100
# 舵机对象
servo0 = servo.Servo(pca.channels[0])
# 超声波测距对象
trig, echo = 10, 9
ultra = Ultrasound(trig, echo)

# varible
angle = 0
step = 1 # 精度 1度
interval = 0.1 # 间隔必须大于0.02 这是测距间隔
s_t = 0 # 开始时间
t = 0 # 时间戳
cycle = 3 # 扫描圈数
# 数据保存
data_arr = []

def end():
    data = pd.DataFrame(data_arr)
    data.columns = ['time', 'angle','distance']
    path = f"/home/pi/Codespace/rasp-pi-project/h5/data/mlida_{int(s_t)}.csv"
    data.to_csv(path, index=None)
    draw(path)
    for i in range(angle, 0, -1): 
        servo0.angle = i
        time.sleep(0.01)
    pca.deinit()
    print("\nend.")

try:
    print("start...")
    s_t = time.time()
    cnt_c = 0
    while cnt_c < cycle:
        t = round(time.time() - s_t, 3)
        servo0.angle = angle
        # 测距
        dis = ultra.get_distance()
        dis = round(dis, 3)
        print("                                      ", end='\r')
        print(t, angle, dis, end='\r')
        data_arr.append((t, angle, dis))
        # 更新角度
        angle += step
        if angle == 180 or angle == 0:
            step = -step
            cnt_c += 1

        time.sleep(interval)
        pass
    end()

except KeyboardInterrupt as e:
    end()
    


